#include "mag_sensor.h"


/******************************************************************************/


uint16_t AS5600_ReadTwoByte(uint16_t readAddr)
{
	uint16_t temp=0xFFFF;                                                                                   
	iic_start();  
	iic_send_byte((0X36<<1)|0x00);    
	iic_wait_ack(); 
	iic_send_byte(readAddr);   
	iic_wait_ack();        
	iic_start();              
	iic_send_byte((0X36<<1)|0x01);              
	iic_wait_ack();     
	temp=iic_read_byte(1);   
	temp=temp<<8|iic_read_byte(0); 
	iic_stop();     
	return temp;
}

unsigned short I2C_getRawCount(void)
{
	return AS5600_ReadTwoByte(0x0C);
}

/******************************************************************************/
//void MagneticSensor_Init(void)
//{
//	cpr=AS5600_CPR;
//	angle_data = angle_data_prev = I2C_getRawCount();  
//	full_rotation_offset = 0;
//	velocity_calc_timestamp=0;
//}
///******************************************************************************/
//float getAngle(void)
//{
//	float d_angle;
//	
//	angle_data = I2C_getRawCount();
//	
//	// tracking the number of rotations 
//	// in order to expand angle range form [0,2PI] to basically infinity
//	d_angle = angle_data - angle_data_prev;
//	// if overflow happened track it as full rotation
//	if(fabs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; 
//	// save the current angle value for the next steps
//	// in order to know if overflow happened
//	angle_data_prev = angle_data;
//	// return the full angle 
//	// (number of full rotations)*2PI + current sensor angle 
//	return  (full_rotation_offset + ( angle_data * 1.0 / cpr) * _2PI) ;
//}
/******************************************************************************/
